package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Victor;

public class BallCollector {
    //motors
    private final Victor 
            topMotor = new Victor(RobotMap.topConveyorMotor), 
            bottomMotor = new Victor(RobotMap.bottomConveyorMotor);
    //Sensors
    private final DigitalInput 
            bottomSensor = new DigitalInput(RobotMap.bottomSensor), 
            topSensor = new DigitalInput(RobotMap.topSensor), 
            shooterSensor = new DigitalInput(RobotMap.shooterSensor);
    
    //DriverStation LCD output        
    private final DriverStationLCD LCD = DriverStationLCD.getInstance();;
    
    //Variables
    private static int ballcount;
    
    public BallCollector(){
        topMotor.setSafetyEnabled(true);
        topMotor.setExpiration(0.5);
        
        bottomMotor.setSafetyEnabled(true);
        bottomMotor.setExpiration(0.5);
    }
    
    public void runConveyor() {
        
        //If there is no ball in shooter, then conveyor will move until there is one.  
        if (shooterSensor.get() == false){
            bottomMotor.set(0.6);
            topMotor.set(0.6);
            ballcount = 0;
        }
        
        //If there is a ball in shooter, 1-Up the ballcount.  Both Conveyors will still move.  
        else if (shooterSensor.get() == true) {
            bottomMotor.set(0.6);
            topMotor.set(0.6);
            ballcount = 1;
        }
        
        //If the top sensor is triggered, and there is a ball in the shooter, ballcount is 2, stop top conveyor
        else if (shooterSensor.get() == true && topSensor.get() == true) {
            topMotor.set(0.0);
            topMotor.set(0.6);
            ballcount = 2;
        }
        
        //If all sensors are triggered, then there are three balls.  Stop both conveyors
        else if (shooterSensor.get() == true && topSensor.get() == true && bottomSensor.get() == true) {
            topMotor.set(0.0);
            bottomMotor.set(0.0);
            ballcount = 3;
        }
        
        
        else {
            LCD.println(DriverStationLCD.Line.kUser2, 0, "Uh Oh...");
            LCD.updateLCD();
        }
    }
    public void stopConveyor() {
        topMotor.set(0.0);
        bottomMotor.set(0.0);
    }
    
        public void manualRunConveyor() {
        topMotor.set(0.6);
        bottomMotor.set(0.6);
    }
    
    //In case ball gets stuck
    public void reverseConveyor() {
        bottomMotor.set(-0.6);
    }
    
    //Self explanatory
    public int getBallCount() {
        LCD.println(DriverStationLCD.Line.kUser2, 0, "Ball Count:" + ballcount);
        LCD.updateLCD();
        return ballcount;
    }
    
    
    //In case something happens. 
   public void resetBallCount() {
       ballcount = 0;
   }
}
